﻿using LaserMark.Control.Service;
using System.Threading;
using static LaserMark.Control.Service.mc;

namespace LaserMark.Extensions;

public static class GoHomeExtension
{
    public static bool Go(short axis, int offset, int backJogVel, int backVel, double acc, double dec)
    {
        int status = 0;
        short capture;
        mc.GT_ZeroPos(0, axis, 1);
        mc.GT_GetDi(0, MC_HOME, out int value);
        var chars = Convert.ToString(value, 2).PadLeft(4, '0');
        var temp = chars.ToList();
        temp.Reverse();
        if (temp[axis - 1] != '1')
        {
            GoHome(axis, backJogVel, acc, dec, out capture);
            if (capture == 1)
            {
                CallBack(axis, offset, backVel, acc, dec, out status);
            }
        }
        return (status & 0x400) == 0;
    }

    private static short GoHome(short axis, int backJogVel, double acc, double dec, out short capture)
    {
        short sRtn;
        sRtn = mc.GT_SetCaptureMode(0, axis, mc.CAPTURE_HOME);
        sRtn = mc.GT_PrfJog(0, axis);
        mc.TJogPrm jogPrm = new TJogPrm();
        jogPrm.acc = acc;
        jogPrm.dec = dec;
        jogPrm.smooth = 0.9;
        sRtn = mc.GT_SetJogPrm(0, axis, ref jogPrm);
        GT_SetVel(0, axis, backJogVel);
        sRtn = mc.GT_Update(0, 1 << (axis - 1));

        do
        {
            Thread.Sleep(2);
            mc.GT_GetCaptureStatus(0, axis, out capture, out _, 1, out _);
        }
        while (capture == 0);

        return sRtn;
    }


    private static int CallBack(short axis, int offset, int backVel, double acc, double dec, out int status)
    {
        short sRtn;
        GT_Stop(0, 1 << (axis - 1), 0);

        Thread.Sleep(500);
        mc.GT_ZeroPos(0, axis, 1);

        sRtn = mc.GT_PrfTrap(0, axis);
        sRtn = mc.GT_GetTrapPrm(0, axis, out TTrapPrm trapPrm);
        trapPrm.acc = acc;
        trapPrm.dec = dec;
        trapPrm.smoothTime = 49;
        sRtn = mc.GT_SetTrapPrm(0, axis, ref trapPrm);
        GT_SetVel(0, axis, backVel);
        GT_SetPos(0, axis, offset);
        sRtn = mc.GT_Update(0, 1 << (axis - 1));

        do
        {
            Thread.Sleep(2);
            sRtn = mc.GT_GetSts(0, axis, out status, 1, out _);
        }
        while ((status & 0x400) != 0);

        if ((status & 0x400) == 0)
        {
            Thread.Sleep(300);
            mc.GT_ZeroPos(0, axis, 1);
        }

        return status;
    }
}
